#include "Constraint.h"
#include "Integrator.h"

void Constraint::satisfyConstraint() const
{
	Vector3& p1_to_p2 = p2->newPosition - p1->newPosition; // vector from p1 to p2

#ifdef OPTIMIZATION
	//optimization avoiding square root
	Real delta = (rest_distance*rest_distance) / p1_to_p2.dotProduct(p1_to_p2) - 1;
	Vector3& correctionVector = p1_to_p2* ( 1 - (1 + 0.5 * delta) ); // 1 + (1/2)delta + (1/8)delta^2 + (1/48)delta^3 
#else
	Real current_distance = p1_to_p2.length(); // current distance between p1 and p2
	Vector3 correctionVector = p1_to_p2*(1 - rest_distance/current_distance); // The offset vector that could moves p1 into a distance of rest_distance to p2
	////Vector3 correctionVectorHalf = correctionVector*0.5; // Lets make it half that length, so that we can move BOTH p1 and p2.
#endif

	if(p1->inverseMass == 0 && p2->inverseMass == 0)
		return;
	else{
		p1->newPosition += (p1->inverseMass / (p1->inverseMass + p2->inverseMass) * correctionVector);
		p2->newPosition -= (p2->inverseMass / (p1->inverseMass + p2->inverseMass) * correctionVector);
	}

	//p1->offsetPos(correctionVectorHalf); // correctionVectorHalf is pointing from p1 to p2, so the length should move p1 half the length needed to satisfy the constraint.
	//p2->offsetPos(-correctionVectorHalf); // we must move p2 the negative direction of correctionVectorHalf since it points from p2 to p1, and not p1 to p2.
}